#include "ifi_picdefs.h"
#include "ifi_default.h"
#include "ifi_aliases.h"
#include "VEXRobot.h"
#include <UserAPI.h>
#include "Delays.h"
#include <stdio.h>

static unsigned char  UltrasonicCount = 0xff;
static unsigned char  UltrasonicInterruptNum = 0;
static unsigned char  UltrasonicInterrupt = 0;
static unsigned char  UltrasonicPort = 0;

unsigned char HiResCounter;
static unsigned char LowResCounter;

static unsigned char SonarDataValid;
static unsigned char fltHiResCounter;
static unsigned char fltLowResCounter;
static unsigned char fltCounter;
static unsigned char Delta;
static unsigned char GotData;

void Handle_EchoPulse_From_Sonar_Device(unsigned char int_hits);
static void Handle_New_Ping(void);

/*******************************************************************************
* FUNCTION NAME: Handle_EchoPulse_From_Sonar_Device
* PURPOSE:       Handle the response from the reciever
* CALLED FROM:   Anywhere
* ARGUMENTS:     void
* RETURNS:       void
*******************************************************************************/
void Handle_EchoPulse_From_Sonar_Device(unsigned char int_hits)
{
   unsigned char   edge;
   if( 0 == (UltrasonicInterrupt & int_hits) )
   {
      return;
   }

   // Get whether this is the rising edge
   if(UltrasonicInterrupt & 0x01)
   {
      edge = INTCON2bits.INTEDG2;
   }
   else if(UltrasonicInterrupt & 0x02)
   {
      edge = INTCON2bits.INTEDG3;
   }

   if (edge)
   {                               /* Yes */
      T0CONbits.T0PS0 = 1;        /* Change scale to DIV 8 (204us delay)*/
      TMR0H = TMR0L = 0;          /* reset timer registers */
      INTCONbits.TMR0IF = 0;      /* make sure int flag is clear */
      INTCONbits.TMR0IE = 1;      /* Enable TMR0 Int */
      if(UltrasonicInterrupt & 0x01)
      {
         INTCON2bits.INTEDG2 = 0;    /* trigger of falling edge */
      }
      else if(UltrasonicInterrupt & 0x02)
      {
         INTCON2bits.INTEDG3 = 0;    /* trigger of falling edge */
      }
      HiResCounter = LowResCounter = 0;
   }
   else
   {
      if(UltrasonicInterrupt & 0x01)
      {
         INTCON3bits.INT2IE = 0; /* disable rc_dig_int1 for now */
      }
      else
      {
         INTCON3bits.INT3IE = 0; /* disable rc_dig_int2 for now */
      }
      LowResCounter = TMR0L;
      INTCONbits.TMR0IE = 0;     /* disable TMR0 Int */

      /* Now apply a filter to data */
      if (HiResCounter < 50)
      {
         if (fltHiResCounter == HiResCounter) 
         {
            SonarDataValid = 1;
            fltCounter = 0;
            Delta = fltLowResCounter - LowResCounter;
            if (Delta & 0x80)   /* If negative then negate it */
               Delta = !Delta;
            if (Delta > 20)
               fltCounter++;
         }
         else
         {
            fltCounter++;
         }

         if (fltCounter > 3)
         {
            SonarDataValid = 1;
            fltHiResCounter = HiResCounter;
            fltLowResCounter = LowResCounter;
         }
      }
   }
}

/*******************************************************************************
* FUNCTION NAME: Handle_New_Ping
* PURPOSE:       Setup a new ping
* CALLED FROM:   Anywhere
* ARGUMENTS:     void
* RETURNS:       void
*******************************************************************************/
static void Handle_New_Ping(void)
{
   // See if currently running
   if( ( 0 == UltrasonicPort ) || ( 0 == UltrasonicInterrupt ) )
   {
      return;
   }

   // See if the data is valid
   if (SonarDataValid)
   {
      SonarDataValid = 0;
      UltrasonicCount = fltHiResCounter;
   }

   if(UltrasonicInterrupt & 0x01)
   {
      INTCON2bits.INTEDG2 = 1;  /* rc_dig_int1 - Interrupt on rising edge */
   }
   else if(UltrasonicInterrupt & 0x02)
   {
      INTCON2bits.INTEDG3 = 1;  /* rc_dig_int2 - Interrupt on rising edge */
   }

   INTCONbits.TMR0IE = 0;    /* make sure TMR0 Int is off */
   T0CONbits.T0PS0 = 0;
   TMR0H = 0;
   INTCONbits.TMR0IF = 0;
   TMR0L = 0xC0;             /* Yeilds a 13us delay */
   SetDigitalOutput(UltrasonicPort, 1); /* prepare a 13us pulse - rising edge of pulse */
   while (!INTCONbits.TMR0IF)
      ;
   SetDigitalOutput(UltrasonicPort, 0); /* falling edge of pulse */
   if(UltrasonicInterrupt & 0x01)
   {
      INTCON3bits.INT2IE = TRUE;  /* enable rc_dig_int1*/
   }
   else if(UltrasonicInterrupt & 0x02)
   {
      INTCON3bits.INT3IE = TRUE;  /* enable rc_dig_int2*/
   }
}

/*******************************************************************************
* FUNCTION NAME: StartUltrasonic
* PURPOSE:       Start the ultrasonic transmitter
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucInterruptNum - Interrupt for recieving the response
*                ucOutputNum - Output to ping
* RETURNS:       void
*******************************************************************************/
void MyStartUltrasonic(unsigned char ucInterruptNum, unsigned char ucOutputNum)
{
   UltrasonicInterrupt = 1 << (ucInterruptNum - 1);
   UltrasonicInterruptNum = ucInterruptNum;
   UltrasonicPort = ucOutputNum;
   SetupDigitalOutput(UltrasonicPort);
   SetupInterruptHandler(UltrasonicInterruptNum, Handle_EchoPulse_From_Sonar_Device);

   T0CON = 0;                /* Timer 0 will be used for the sonar sensor */
   T0CONbits.T08BIT = 1;     /* Make it an 8 bit timer */
   T0CONbits.T0PS0 = 0;      // 1:2
   T0CONbits.T0PS1 = 0;
   T0CONbits.T0PS2 = 0;
   T0CONbits.PSA = 0;
   T0CONbits.TMR0ON = 1;     /* Turn it on */
   INTCONbits.TMR0IE = FALSE;    /* disable TMR0 ints for now */
   INTCON2bits.TMR0IP = 0;   /* Make TMR0 low priority */
   INTCON3bits.INT2IE = 0;   /* disable rc_dig_int1 for now */

   Handle_New_Ping();
}

/*******************************************************************************
* FUNCTION NAME: StopUltrasonic
* PURPOSE:       Stop the ultrasonic transmitter
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucInterruptNum - Interrupt for recieving the response
*                ucOutputNum - Output to ping
* RETURNS:       void
*******************************************************************************/
void MyStopUltrasonic()
{
   UltrasonicInterrupt = 0;
   UltrasonicPort = 0;
}

/*******************************************************************************
* FUNCTION NAME: GetUltrasonic
* PURPOSE:       Get the current distance
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucInterruptNum - Interrupt for recieving the response
*                ucOutputNum - Output to ping
* RETURNS:       unsigned char
*******************************************************************************/
unsigned char MyGetUltrasonic()
{
   return UltrasonicCount;
}

int MyUltrasonicReady(void)
{
   return SonarDataValid;
}
